#include "CPU.h"
#include "CPUOpCodes.h"
#include "Log.h"
#include <iomanip>

PNE_NAMESPACE_BEGIN

CPU::CPU(MainBus &mem)
    : m_pendingNMI(false),
      m_pendingIRQ(false),
      m_bus(mem) {}

void CPU::Reset()
{
    Reset(readAddress(ResetVector));
}

void CPU::Reset(Address start_addr)
{
    m_skipCycles = m_cycles = 0;
    r_A = r_X = r_Y = 0;
    f_I = true;
    f_C = f_D = f_N = f_V = f_Z = false;
    r_PC = start_addr;
    r_SP = 0xfd; // documented startup state
}

void CPU::Interrupt(InterruptType type)
{
    switch (type)
    {
    case InterruptType::NMI:
        m_pendingNMI = true;
        break;

    case InterruptType::IRQ:
        m_pendingIRQ = true;
        break;
    default:
        break;
    }
}

void CPU::interruptSequence(InterruptType type)
{
    if (f_I && type != InterruptType::NMI && type != InterruptType::BRK_)
        return;
    if (type == InterruptType::BRK_)
        ++r_PC;

    pushStack(r_PC >> 8);
    pushStack(r_PC);
    Byte flags = static_cast<Byte>((int)f_N << 7 |
                                   (int)f_V << 6 |
                                   1 << 5 | // unused bit, supposed to be always 1
                                   (type == InterruptType::BRK_) << 4 |
                                   (int)f_D << 3 |
                                   (int)f_I << 2 |
                                   (int)f_Z << 1 |
                                   (int)f_C);

    pushStack(flags);
    f_I = true;

    switch (type)
    {
    case InterruptType::IRQ:
    case InterruptType::BRK_:
        r_PC = readAddress(IRQVector);
        break;
    case InterruptType::NMI:
        r_PC = readAddress(NMIVector);
        break;
    }

    // Interrupt sequence takes 7, but one cycle was actually spent on this.
    // So skip 6
    m_skipCycles += 6;
}

void CPU::pushStack(Byte value)
{
    m_bus.Write(0x100 | r_SP, value);
    --r_SP;
}

Byte CPU::pullStack()
{
    return m_bus.Read(0x100 | ++r_SP);
}

void CPU::setZN(Byte value)
{
    f_Z = !value;
    f_N = value & 0x80;
}

void CPU::setPageCrossed(Address a, Address b, int inc)
{
    // Page is determined by the high byte
    if ((a & 0xff00) != (b && 0xff00))
        m_skipCycles += inc;
}

void CPU::SkipDMACycles()
{
    m_skipCycles += 513;            // 256 read + 256 write + 1 dummy read
    m_skipCycles += (m_cycles & 1); // +1 on odd cycle
}

void CPU::Step()
{
    ++m_cycles;
    if (m_skipCycles-- > 1)
        return;
    m_skipCycles = 0;

    // NMI has higher priority, check for it first
    if (m_pendingNMI)
    {
        interruptSequence(InterruptType::NMI);
        m_pendingNMI = m_pendingIRQ = false;
        return;
    }
    else if (m_pendingIRQ)
    {
        interruptSequence(InterruptType::IRQ);
        m_pendingNMI = m_pendingIRQ = false;
        return;
    }

    int psw = static_cast<int>(f_N << 7 |
                               f_V << 6 |
                               1 << 5 |
                               f_D << 1 |
                               f_I << 2 |
                               f_Z << 1 |
                               static_cast<int>(f_C));

    LOG_CPU << std::hex << std::setfill('0') << std::uppercase
            << std::setw(4) << +r_PC << " " << std::setw(2) << +m_bus.Read(r_PC)
            << " " << "A:" << std::setw(2) << +r_A << " "
            << "X:" << std::setw(2) << +r_A << " "
            << "Y:" << std::setw(2) << +r_Y << " "
            << "P:" << std::setw(2) << psw << " "
            << "SP:" << std::setw(2) << +r_SP << " "
            << "CYC:" << std::setw(3) << std::setfill(' ')
            << std::dec << ((m_cycles - 1) * 3) % 341
            << std::endl;

    Byte opcode = m_bus.Read(r_PC++);

    auto CycleLenght = OperationCycles[opcode];

    // using short - circuit evaluation, call the other function only if
    // the first failed
    // ExecuteImplied must be called first and ExecuteBranch must be before
    // ExecuteType0
    if (CycleLenght &&
        (executeImplied(opcode) ||
         executeBranch(opcode) ||
         executeType1(opcode) ||
         executeType2(opcode) ||
         executeType0(opcode)))
    {
        m_skipCycles += CycleLenght;
    }
    else
    {
        LOG(Level::Error) << "Unrecognized opcode: " << std::hex
                          << +opcode << std::endl;
    }
}

bool CPU::executeImplied(Byte opcode)
{
    switch (static_cast<OperationImplied>(opcode))
    {
    case OperationImplied::NOP:
        break;
    case OperationImplied::BRK:
        interruptSequence(InterruptType::BRK_);
        break;
    case OperationImplied::JSR:
        pushStack(static_cast<Byte>((r_PC + 1) >> 8));
        pushStack(static_cast<Byte>(r_PC + 1));
        break;
    case OperationImplied::RTS:
        r_PC = pullStack();
        r_PC |= pullStack() << 8;
        ++r_PC;
        break;
    case OperationImplied::RTI:
    {
        Byte flags = pullStack();
        f_N = flags & 0x80;
        f_V = flags & 0x40;
        f_D = flags & 0x8;
        f_I = flags & 0x4;
        f_Z = flags & 0x2;
        f_C = flags & 0x1;
    }

        r_PC = pullStack();
        r_PC |= pullStack() << 8;
        break;
    case OperationImplied::JMP:
        r_PC = readAddress(r_PC);
        break;
    case OperationImplied::JMPI:
    {
        Address location = readAddress(r_PC);
        Address Page = location & 0xff00;
        r_PC = m_bus.Read(location) | m_bus.Read(Page |
                                                 ((location + 1) & 0xff))
                                          << 8;
    }
    break;
    case OperationImplied::PHP:
    {
        Byte flags = static_cast<Byte>(f_N << 7 |
                                       f_V << 6 |
                                       1 << 5 | // supposed to always be 1
                                       1 << 4 | // PHP pushes with the B flag as 1, no matter what
                                       f_D << 3 |
                                       f_I << 2 |
                                       f_Z << 1 |
                                       static_cast<int>(f_C));
        pushStack(flags);
    }
    case OperationImplied::PLP:
    {
        Byte flags = pullStack();
        f_N = flags & 0x80;
        f_V = flags & 0x40;
        f_D = flags & 0x8;
        f_I = flags & 0x4;
        f_Z = flags & 0x2;
        f_C = flags & 0x1;
    }
    break;
    case OperationImplied::PHA:
        pushStack(r_A);
        break;
    case OperationImplied::PLA:
        r_A = pullStack();
        setZN(r_A);
        break;
    case OperationImplied::DEY:
        --r_Y;
        setZN(r_Y);
        break;
    case OperationImplied::DEX:
        --r_X;
        setZN(r_X);
        break;
    case OperationImplied::TAY:
        r_Y = r_A;
        setZN(r_Y);
        break;
    case OperationImplied::INY:
        ++r_Y;
        setZN(r_Y);
        break;
    case OperationImplied::INX:
        ++r_X;
        setZN(r_X);
        break;
    case OperationImplied::CLC:
        f_C = false;
        break;
    case OperationImplied::SEC:
        f_C = true;
        break;
    case OperationImplied::CLI:
        f_I = false;
        break;
    case OperationImplied::SEI:
        f_I = true;
        break;
    case OperationImplied::CLD:
        f_D = false;
        break;
    case OperationImplied::SED:
        f_D = true;
        break;
    case OperationImplied::TYA:
        r_A = r_Y;
        setZN(r_A);
        break;
    case OperationImplied::CLV:
        f_V = false;
        break;
    case OperationImplied::TXA:
        r_A = r_X;
        setZN(r_A);
        break;
    case OperationImplied::TXS:
        r_SP = r_X;
        break;
    case OperationImplied::TAX:
        r_X = r_A;
        setZN(r_X);
        break;
    case OperationImplied::TSX:
        r_X = r_SP;
        setZN(r_X);
        break;
    default:
        return false;
    }

    return true;
}

bool CPU::executeBranch(Byte opcode)
{
    if ((opcode & BranchInstructionMask) == BranchInstructionMaskResult)
    {
        // branch is initialized to the condition required (for the flag specified later)
        bool branch = opcode & BranchConditionMask;

        // set branch to true if the given condition is met by the given flag
        // We use xnor here, it is true if either both operands are true or false
        switch (opcode >> BranchOnFlagShift)
        {
        case static_cast<int>(BranchOnFlags::Negative):
            branch = !(branch ^ f_N);
            break;
        case static_cast<int>(BranchOnFlags::Overflow):
            branch = !(branch ^ f_V);
            break;
        case static_cast<int>(BranchOnFlags::Carry):
            branch = !(branch ^ f_C);
            break;
        case static_cast<int>(BranchOnFlags::Zero):
            branch = !(branch ^ f_Z);
            break;
        default:
            return false;
        }

        if (branch)
        {
            int8_t offset = m_bus.Read(r_PC++);
            ++m_skipCycles;
            auto newPC = static_cast<Address>(r_PC + offset);
            setPageCrossed(r_PC, newPC, 2);
            r_PC = newPC;
        }
        else
            ++r_PC;
        return true;
    }
    return false;
}

bool CPU::executeType1(Byte opcode)
{
    if ((opcode & InstructionModeMask) == 0x1)
    {
        Address location = 0; // Location of the operand, could be in RAM
        auto op = static_cast<OperationOne>((opcode & OperationMask) >> OperationShift);
        switch (static_cast<AddrModeOne>(
            (opcode & AddrModeMask) >> AddrModeShift))
        {
        case AddrModeOne::IndexedIndirectX:
        {
            Byte zero_addr = r_X + m_bus.Read(r_PC++);
            // Addresses wrap in zero page mode, thus pass through a mask
            location = m_bus.Read(zero_addr & 0xff) | m_bus.Read((zero_addr + 1) & 0xff) << 8;
        }
        break;
        case AddrModeOne::ZeroPage:
            location = m_bus.Read(r_PC++);
            break;
        case AddrModeOne::Immediate:
            location = r_PC++;
            break;
        case AddrModeOne::Absolute:
            location = readAddress(r_PC);
            r_PC += 2;
            break;
        case AddrModeOne::IndirectY:
        {
            Byte zero_addr = m_bus.Read(r_PC++);
            location = m_bus.Read(zero_addr & 0xff) | m_bus.Read((zero_addr + 1) & 0xff) << 8;
            if (op != OperationOne::STA)
                setPageCrossed(location, location + r_Y);
            location += r_Y;
        }
        break;
        case AddrModeOne::IndexedX:
            // Address wraps around in the zero page
            location = (m_bus.Read(r_PC++) + r_X) & 0xff;
            break;
        case AddrModeOne::AbsoluteY:
            location = readAddress(r_PC);
            r_PC += 2;
            if (op != OperationOne::STA)
                setPageCrossed(location, location + r_Y);
            location += r_Y;
            break;
        case AddrModeOne::AbsoluteX:
            location = readAddress(r_PC);
            r_PC += 2;
            if (op != OperationOne::STA)
                setPageCrossed(location, location + r_X);
            location += r_X;
            break;
        default:
            return false;
        }

        switch (op)
        {
        case OperationOne::ORA:
            r_A |= m_bus.Read(location);
            setZN(r_A);
            break;
        case OperationOne::AND:
            r_A &= m_bus.Read(location);
            setZN(r_A);
            break;
        case OperationOne::EOR:
            r_A ^= m_bus.Read(location);
            setZN(r_A);
            break;
        case OperationOne::ADC:
        {
            Byte operand = m_bus.Read(location);
            std::uint16_t sum = r_A + operand + f_C;
            // Carry forward or UNSIGNED overflow
            f_C = sum & 0x100;
            // SIGNED overflow, would only happen if the sign of sum is
            // different from BOTH the operands
            f_V = (r_A ^ sum) & (operand ^ sum) & 0x80;
            r_A = static_cast<Byte>(sum);
            setZN(r_A);
        }
        break;
        case OperationOne::STA:
            m_bus.Write(location, r_A);
            break;
        case OperationOne::LDA:
            r_A = m_bus.Read(location);
            setZN(r_A);
            break;
        case OperationOne::SBC:
        {
            // High carry means "no borrow", thus negate and subtract
            std::uint16_t subtrahend = m_bus.Read(location),
                          diff = r_A - subtrahend - !f_C;
            // if the ninth bit is 1, the resulting number is negative => borrow => low carry
            f_C = !(diff & 0x100);
            // Same as ADC, except instead of the subtrahend,
            // substitute with it's one complement
            f_V = (r_A ^ diff) & (~subtrahend ^ diff) & 0x80;
            r_A = diff;
            setZN(diff);
        }
        break;
        case OperationOne::CMP:
        {
            std::uint16_t diff = r_A - m_bus.Read(location);
            f_C = !(diff & 0x100);
            setZN(diff);
        }
        break;
        default:
            return false;
        }
        return true;
    }
    return false;
}

bool CPU::executeType2(Byte opcode)
{
    if ((opcode & InstructionModeMask) == 2)
        {
            Address location = 0;
            auto op = static_cast<OperationTwo>((opcode & OperationMask) >> OperationShift);
            auto addr_mode =
                    static_cast<AddrModeTwo>((opcode & AddrModeMask) >> AddrModeShift);
            switch (addr_mode)
            {
                case AddrModeTwo::Immediate_:
                    location = r_PC++;
                    break;
                case AddrModeTwo::ZeroPage_:
                    location = m_bus.Read(r_PC++);
                    break;
                case AddrModeTwo::Accumulator:
                    break;
                case AddrModeTwo::Absolute_:
                    location = readAddress(r_PC);
                    r_PC += 2;
                    break;
                case AddrModeTwo::Indexed:
                    {
                        location = m_bus.Read(r_PC++);
                        Byte index;
                        if (op == OperationTwo::LDX || op == OperationTwo::STX)
                            index = r_Y;
                        else
                            index = r_X;
                        //The mask wraps address around zero page
                        location = (location + index) & 0xff;
                    }
                    break;
                case AddrModeTwo::AbsoluteIndexed:
                    {
                        location = readAddress(r_PC);
                        r_PC += 2;
                        Byte index;
                        if (op ==  OperationTwo::LDX || op ==  OperationTwo::STX)
                            index = r_Y;
                        else
                            index = r_X;
                        setPageCrossed(location, location + index);
                        location += index;
                    }
                    break;
                default:
                    return false;
            }

            std::uint16_t operand = 0;
            switch (op)
            {
                case  OperationTwo::ASL:
                case  OperationTwo::ROL:
                    if (addr_mode == AddrModeTwo::Accumulator)
                    {
                        auto prev_C = f_C;
                        f_C = r_A & 0x80;
                        r_A <<= 1;
                        //If Rotating, set the bit-0 to the the previous carry
                        r_A = r_A | (prev_C && (op ==  OperationTwo::ROL));
                        setZN(r_A);
                    }
                    else
                    {
                        auto prev_C = f_C;
                        operand = m_bus.Read(location);
                        f_C = operand & 0x80;
                        operand = operand << 1 | (prev_C && (op ==  OperationTwo::ROL));
                        setZN(operand);
                        m_bus.Write(location, operand);
                    }
                    break;
                case  OperationTwo::LSR:
                case  OperationTwo::ROR:
                    if (addr_mode == AddrModeTwo::Accumulator)
                    {
                        auto prev_C = f_C;
                        f_C = r_A & 1;
                        r_A >>= 1;
                        //If Rotating, set the bit-7 to the previous carry
                        r_A = r_A | (prev_C && (op ==  OperationTwo::ROR)) << 7;
                        setZN(r_A);
                    }
                    else
                    {
                        auto prev_C = f_C;
                        operand = m_bus.Read(location);
                        f_C = operand & 1;
                        operand = operand >> 1 | (prev_C && (op ==  OperationTwo::ROR)) << 7;
                        setZN(operand);
                        m_bus.Write(location, operand);
                    }
                    break;
                case  OperationTwo::STX:
                    m_bus.Write(location, r_X);
                    break;
                case  OperationTwo::LDX:
                    r_X = m_bus.Read(location);
                    setZN(r_X);
                    break;
                case  OperationTwo::DEC:
                    {
                        auto tmp = m_bus.Read(location) - 1;
                        setZN(tmp);
                        m_bus.Write(location, tmp);
                    }
                    break;
                case  OperationTwo::INC:
                    {
                        auto tmp = m_bus.Read(location) + 1;
                        setZN(tmp);
                        m_bus.Write(location, tmp);
                    }
                    break;
                default:
                    return false;
            }
            return true;
        }
        return false;
}

bool CPU::executeType0(Byte opcode)
{
    if ((opcode & InstructionModeMask) == 0x0)
        {
            Address location = 0;
            switch (static_cast<AddrModeTwo>((opcode & AddrModeMask) >> AddrModeShift))
            {
                case AddrModeTwo::Immediate_:
                    location = r_PC++;
                    break;
                case AddrModeTwo::ZeroPage_:
                    location = m_bus.Read(r_PC++);
                    break;
                case AddrModeTwo::Absolute_:
                    location = readAddress(r_PC);
                    r_PC += 2;
                    break;
                case AddrModeTwo::Indexed:
                    // Address wraps around in the zero page
                    location = (m_bus.Read(r_PC++) + r_X) & 0xff;
                    break;
                case AddrModeTwo::AbsoluteIndexed:
                    location = readAddress(r_PC);
                    r_PC += 2;
                    setPageCrossed(location, location + r_X);
                    location += r_X;
                    break;
                default:
                    return false;
            }
            std::uint16_t operand = 0;
            switch (static_cast<OperationZero>((opcode & OperationMask) >> OperationShift))
            {
                case OperationZero::BIT:
                    operand = m_bus.Read(location);
                    f_Z = !(r_A & operand);
                    f_V = operand & 0x40;
                    f_N = operand & 0x80;
                    break;
                case OperationZero::STY:
                    m_bus.Write(location, r_Y);
                    break;
                case OperationZero::LDY:
                    r_Y = m_bus.Read(location);
                    setZN(r_Y);
                    break;
                case OperationZero::CPY:
                    {
                        std::uint16_t diff = r_Y - m_bus.Read(location);
                        f_C = !(diff & 0x100);
                        setZN(diff);
                    }
                    break;
                case OperationZero::CPX:
                    {
                        std::uint16_t diff = r_X - m_bus.Read(location);
                        f_C = !(diff & 0x100);
                        setZN(diff);
                    }
                    break;
                default:
                    return false;
            }

            return true;
        }
        return false;
}

Address CPU::readAddress(Address addr)
{
    return m_bus.Read(addr) | m_bus.Read(addr + 1) << 8;
}

PNE_NAMESPACE_END // namespace
// DONE